#include "MCU_PORT.h"

uint8_t TCK_RxProBuf[100];
uint8_t DBG_RxProBuf[100];
uint8_t CTL_RxProBuf[100];

#ifdef MCU_STM32F103RCT6
void mcuInit(void)
{
	//开启串口中断
	__HAL_UART_ENABLE_IT(&huart1,UART_IT_IDLE);
	Uart_RxAgain(&huart1);
	__HAL_UART_ENABLE_IT(&huart4,UART_IT_IDLE);
	Uart_RxAgain(&huart4);
	__HAL_UART_ENABLE_IT(&huart5,UART_IT_IDLE);
	Uart_RxAgain(&huart5);
	//开启编码器
	__HAL_TIM_SET_COUNTER(&htim1,0);
	__HAL_TIM_SET_COUNTER(&htim5,0);
	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
	HAL_TIM_Encoder_Start(&htim5,TIM_CHANNEL_ALL);
	//开启定时器中断
	HAL_TIM_Base_Start_IT((TIM_HandleTypeDef *)&htim3);
	HAL_TIM_Base_Start_IT((TIM_HandleTypeDef *)&htim4);
	HAL_TIM_Base_Start_IT((TIM_HandleTypeDef *)&htim6);
	
}

void getMotorVelocity(int* left,int* right)
{
	*left  = -(short)(__HAL_TIM_GET_COUNTER(&htim5));
	*right  = (short)(__HAL_TIM_GET_COUNTER(&htim1));
	__HAL_TIM_SET_COUNTER(&htim1,0);
	__HAL_TIM_SET_COUNTER(&htim5,0);
}

void debugUartSend(uint8_t* data,int len)
{
	HAL_UART_Transmit(&huart5,data,len,50);
}
void ctrlUartSend(uint8_t* data)
{
	HAL_UART_Transmit(&huart1,data,strlen(data),50);
}
void trackUartSend(uint8_t* data,int len)
{
	HAL_UART_Transmit(&huart4,data,len,50);
}

void Uart_RxAgain(UART_HandleTypeDef *huart)
{
	if(huart->Instance==USART1)
	{
		HAL_UARTEx_ReceiveToIdle_IT(huart,CTL_RxProBuf,100);
	}
	else if(huart->Instance==UART4)
	{
		HAL_UARTEx_ReceiveToIdle_IT(huart,TCK_RxProBuf,100);
	}
	else if(huart->Instance==UART5)
	{
		HAL_UARTEx_ReceiveToIdle_IT(huart,DBG_RxProBuf,100);
	}
}

#endif
extern char xtt[100];
#ifdef MCU_TIVAC123
void mcuInit(void)
{
	//-外设初始化
	//--串口初始化
	Usart0_init(115200);
	Usart1_init(115200);
	Usart5_init(115200);
	//--OLED初始化
	OLED_Init();
	//--编码器初始化
	QEI0_init();
	QEI1_init();
	//--定时器初始化
	Time0A_init();
	Time1A_init();
	Time2A_init();
	//--GPIO初始化
	//---方向引脚初始化
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
	GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_4);//BIN1
	GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_7);//BIN2
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
	GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_2);//AIN1
	GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_3);//AIN2
	//---STBY引脚初始化
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
	GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_2);//STBY
	GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_2, 1 << 2);
	//--PWM初始化
	PWM_Init(1000); 
	//--LED初始化
	Led_init();
}

void getMotorVelocity(int* left,int* right)
{
	*left  = (short)QEI0_get();
	*right  = (short)QEI1_get();
}

void debugUartSend(uint8_t* data,int len)
{
	Usart0_txpacket(data);
}
void ctrlUartSend(uint8_t* data)
{
	Usart1_txpacket(data);
}
void trackUartSend(uint8_t* data,int len)
{
	Usart5_txpacket(data);
}
#endif

void tim10msISR(void)
{
	#ifdef MCU_TIVAC123
	TimerIntClear(TIMER0_BASE,TIMER_TIMA_TIMEOUT);
	#endif
	//编码测速
	getMotorVelocity(&LeftMT.PVreality,&RightMT.PVreality);
	lsendspd+=LeftMT.PVreality;
	rsendspd+=RightMT.PVreality;
	//路程累计
	LeftMT.PPreality += LeftMT.PVreality;
	RightMT.PPreality += RightMT.PVreality;
	//循迹控制
	track_control();
	//速度循迹控制
	speed_control();
	// //状态检测
	// motor_status_update();
	//路程控制 
	postition_control();
}

void tim50msISR(void)
{
	#ifdef MCU_TIVAC123
		TimerIntClear(TIMER1_BASE,TIMER_TIMA_TIMEOUT);
	#endif
}

void tim100msISR(void)
{
	#ifdef MCU_TIVAC123
	  TimerIntClear(TIMER2_BASE,TIMER_TIMA_TIMEOUT);
	#endif
	char txt[40];
	//调试串口数据发送
	//BLT_Send_WaveData(lsendspd/10.0,LeftMT.PVtarget,rsendspd/10.0,RightMT.PVtarget,LeftMT.PPreality,LeftMT.PPtarget,RightMT.PPreality,RightMT.PPtarget);
	//BLT_Send_WaveData(LeftMT.PVreality,LeftMT.PVtarget,RightMT.PVreality,RightMT.PVtarget,LeftMT.PPreality,LeftMT.PPtarget,RightMT.PPreality,RightMT.PPtarget);
	lsendspd=0;
	rsendspd=0;
	//状态计数器更新
	stactr++;
	stactr%=20;
}

void ctrlUartISR(void)
{
	ctrolcmd_process();
}

void debugUartISR(void)
{
	debugcmd_process();
}

void trackUartISR(void)
{
	trackcmd_process();
}